//#define USE_SOFTWARE_SPI

using GT = Gadgeteer;
using GTI = Gadgeteer.Interfaces;
using GTM = Gadgeteer.Modules;

namespace MLRobotic.Asserv.Drivers
{
    /**********************************************************************
    // Routine : LS7366 class
    //---------------------------------------------------------------------
    // Date : 20110516
    // Name : Wim Cranen
    //---------------------------------------------------------------------
    // Version : v4.0
    //---------------------------------------------------------------------
    // History : v1.0 
    //         : v2.0 Clear and config
    //         : v3.0 Class extended with read CNTR function
    //         : v4.0 Made class smaller and more compact
    //         : v5.0 Adaptation for gadgeteer
    **********************************************************************/

    /*******************************************************************
    // LS7366 REGISTERS
    //------------------------------------------------------------------
    // MDR0 : MoDe Register 0 (8 bit read/write)
    //        Upon power up, MDR0 is set to zero
    //        B1 B0: count mode 1
    //            00 = non quadrature mode (A = clock, B = direction)
    //            01 = x1 quadrature mode (one count per quad. cycle)
    //            10 = x2 quadrature mode (two counts per quad. cycle)
    //            11 = x4 quadrature mode (four counts per quad. cycle)
    //        B3 B2: count mode 2
    //            00 = free running count mode
    //            01 = single-cycle count mode (disabled on carry or
    //                 borrow, resumed on reset)
    //            10 = range limit count mode (up and down between DTR
    //                 and zero, freezes at limit and resumes on 
    //                 direction change
    //            11 = modulo-n count, where n = DTR in both directions
    //        B5 B4: index function
    //            00 = 
    //            01 =
    //            10 =
    //            11 =
    //        B6:  0 = asynchronous index
    //             1 = synchronous index (overriden in non-quad. mode)
    //        B7:  0 = filter clock division factor = 1
    //             1 = filter clock division factor = 2
    //------------------------------------------------------------------
    // MDR1 : MoDe Register 1 (8 bit read/write) as appendix to MDR0
    //        Upon power up, MDR0 is set to zero
    //        B1 B0: count mode 1
    //            00 = 4-byte counter mode
    //            01 = 3-byte counter mode
    //            10 = 2-byte counter mode
    //            11 = 1-byte counter mode
    //        B2:  0 = enable counting
    //             1 = disable counting
    //        B3:  not used
    //        B4:  0 = NOP
    //             1 = FLAG on IDX (B4 of STR) \
    //        B5:  0 = NOP                      |
    //             1 = FLAG on CMP (B5 of STR)  |  NOTE: applicable on
    //        B6:  0 = NOP                       > both LFLAG and DFLAG
    //             1 = FLAG on BW (B6 of STR)   |
    //        B7:  0 = NOP                      |
    //             1 = FLAG on CY (B7 of STR)  /
    //------------------------------------------------------------------
    // DTR  : DaTa Register input (8, 16, 24 or 32 bits)
    //        Can be written directly from MOSI
    //------------------------------------------------------------------
    // CNTR : CouNTer Register (8, 16, 24 or 32 bits)
    //        This register counts the up/down pulses from the quad.
    //        clock applied at the A and B inputs or alternatively
    //        from the A input in non-quad. mode
    //------------------------------------------------------------------
    // OTR  : OuTput Register (8, 16, 24 or 32 bits)
    //        Shadow register from CNTR to read the counter value
    //        during counting. Can be read directly from MOSI while
    //        the counter still counts
    //------------------------------------------------------------------
    // STR  : STatus Register (8 bit read)
    //        B0: S   = signbit (0 neg, 1 pos)
    //        B1: U/D = up/down indicator (0 down, 1 up)
    //        B2: PLS = power loss indicator latch; set upon powerup
    //        B3: CEN = count enable status (0 disabled, 1 enabled)
    //        B4: IDX = index latch
    //        B5: CMP = compare (CNTR = DTR) latch
    //        B6: BW  = borrow (CNTR underlow) latch
    //        B7: CY  = carry (CNTR overflow) latch
    //------------------------------------------------------------------
    // IR   : Instruction Register (8 bit write)
    //        B2 B1 B0 : XXX (don't care)
    //        B5 B4 B3 : 000 = select none
    //                   001 = select MDR0
    //                   010 = select MDR1
    //                   011 = select DTR
    //                   100 = select CNTR
    //                   101 = select OTR
    //                   110 = select STR
    //                   111 = select none
    //           B7 B6 :  00 = CLR register
    //                    01 = RD register
    //                    10 = WR register
    //                    11 = LOAD register
    /*******************************************************************
    // Number of Bytes | OP Code | Register |  Operation
    //-----------------+---------+----------+---------------------------
    //                 |         | MDR0     | Clear MDR0 to zero
    //                 |         | MDR1     | Clear MDR1 to zero
    //       1         |  CLR    | DTR      | None
    //                 |         | CNTR     | Clear CNTR to zero
    //                 |         | OTR      | None
    //                 |         | STR      | Clear STR to zero
    //-----------------+---------+----------+---------------------------             
    //                 |         | MDR0     | Output MDR0 on MISO
    //                 |         | MDR1     | Output MDR1 on MISO
    //    2 to 5       |  RD     | DTR      | None
    //                 |         | CNTR     | Transfer CNTR to OTR, 
    //                 |         |          | then output OTR on MISO
    //                 |         | OTR      | Output OTR on MISO
    //                 |         | STR      | Output STR on MISO
    //-----------------+---------+----------+---------------------------
    //                 |         | MDR0     | Write at MOSI into MDR0
    //                 |         | MDR1     | Write at MOSI into MDR1
    //    2 to 5       |  WR     | DTR      | Write at MOSI into DTR
    //                 |         | CNTR     | None
    //                 |         | OTR      | None
    //                 |         | STR      | None
    //-----------------+---------+----------+---------------------------
    //                 |         | MDR0     | None
    //                 |         | MDR1     | None
    //       1         |  LOAD   | DTR      | None
    //                 |         | CNTR     | Transfer DTR to CNTR
    //                 |         | OTR      | Transfer CNTR to OTR
    *******************************************************************/

    // -- CHANGE FOR MICRO FRAMEWORK 4.2 --
    // If you want to use Serial, SPI, or DaisyLink (which includes GTI.SoftwareI2C), you must do a few more steps
    // since these have been moved to separate assemblies for NETMF 4.2 (to reduce the minimum memory footprint of Gadgeteer)
    // 1) add a reference to the assembly (named Gadgeteer.[interfacename])
    // 2) in GadgeteerHardware.xml, uncomment the lines under <Assemblies> so that end user apps using this module also add a reference.

    public class PulseCount : GTM.Module
    {
        #region Buffers
        // Read/write buffers
        byte[] LS7366_1B_wr = new byte[1]; // write one byte
        byte[] LS7366_2B_wr = new byte[2]; // write two bytes
        byte[] LS7366_3B_wr = new byte[5]; // write three bytes
        byte[] LS7366_2B_rd = new byte[2]; // read two bytes
        byte[] LS7366_4B_rd = new byte[4]; // read four bytes
        byte[] LS7366_5B_rd = new byte[5]; // read four bytes
        #endregion Buffers

        #region SPI
#if USE_SOFTWARE_SPI

        private GTI.DigitalInput MISO;
        private GTI.DigitalOutput MOSI;
        private GTI.DigitalOutput CLOCK;
        private GTI.DigitalOutput CS;
#else
        private readonly GTI.SPI.Configuration spiConfig;
        private readonly GTI.SPI spi;
#endif
        #endregion

        public PulseCount(int socketNumber)
        {
            GT.Socket socket = GT.Socket.GetSocket(socketNumber, true, this, null);

#if USE_SOFTWARE_SPI
            socket.EnsureTypeIsSupported('Y', this);

            CS = new GTI.DigitalOutput(socket, GT.Socket.Pin.Six, true, this);
            MISO = new GTI.DigitalInput(socket, GT.Socket.Pin.Eight, GTI.GlitchFilterMode.Off, GTI.ResistorMode.Disabled, this);
            MOSI = new GTI.DigitalOutput(socket, GT.Socket.Pin.Seven, false, this);
            CLOCK = new GTI.DigitalOutput(socket, GT.Socket.Pin.Nine, false, this);
#else
            socket.EnsureTypeIsSupported('S', this);

            spiConfig = new GTI.SPI.Configuration(false, 0, 0, false, true, 500);
            spi = new GTI.SPI(socket, spiConfig, GTI.SPI.Sharing.Shared, socket, GT.Socket.Pin.Six, this);
#endif
            
            Init();
        }

        public int ReadCounter()
        {
            //LS7366_1B_rd = Return1Byte((byte)Commands.LS7366_READ | (byte)Registers.LS7366_STR);
            return Return4Bytes((byte)Commands.LS7366_READ | (byte)Registers.LS7366_CNTR);
        }

        public void ResetCounter()
        {
            // Clear STR register
            Write1Byte((byte)Commands.LS7366_CLEAR | (byte)Registers.LS7366_STR);

            // Clear CNTR
            Write1Byte((byte)Commands.LS7366_CLEAR | (byte)Registers.LS7366_CNTR);

            // Clear ORT (write CNTR into OTR)
            Write1Byte((byte)Commands.LS7366_LOAD | (byte)Registers.LS7366_OTR);
        }

        public void Init()
        {
            ClearRegisters();

            // Configure MDR0 register
            Write2Bytes((byte)Commands.LS7366_WRITE                 // write command
                             | (byte)Registers.LS7366_MDR0,         // to MDR0
                               (byte)MDR0Mode.LS7366_MDR0_QUAD4     // quadrature mode
                             | (byte)MDR0Mode.LS7366_MDR0_FREER     // free run counting
                             | (byte)MDR0Mode.LS7366_MDR0_DIDX);    // disable index
            
            // Configure MDR1 register
            Write2Bytes((byte)Commands.LS7366_WRITE                 // write command
                             | (byte)Registers.LS7366_MDR1,         // to MDR1
                               (byte)MDR1Mode.LS7366_MDR1_4BYTE     // 4 byte counter mode
                             | (byte)MDR1Mode.LS7366_MDR1_ENCNT);   // enable counting

            // Configure DTR register to 15 ticks/revolution
            // -- The encoder produces 6000 ticks/revolution
            // -- We want 400 measurements per revolution
            // -- 6000/400 = 15 (subtract 1 because counting starts at ZERO[=binairy count])
            // -- Together with the setting of MDR1Mode.LS7366_MDR1_FLCMP in MDR1
            // -- It enables the DFLAG output (pin 9 on LS7366) to go LOW while CNTR equals DTR
            //LS7366.ConfigDTR2Bytes(14);
        }

        private void ClearRegisters()
        {
            // Clear MDR0 register
            Write1Byte((byte)Commands.LS7366_CLEAR | (byte)Registers.LS7366_MDR0);
            // Clear MDR1 register
            Write1Byte((byte)Commands.LS7366_CLEAR | (byte)Registers.LS7366_MDR1);
            // Clear STR register
            Write1Byte((byte)Commands.LS7366_CLEAR | (byte)Registers.LS7366_STR);
            // Clear CNTR
            Write1Byte((byte)Commands.LS7366_CLEAR | (byte)Registers.LS7366_CNTR);
            // Clear ORT (write CNTR into OTR)
            Write1Byte((byte)Commands.LS7366_LOAD | (byte)Registers.LS7366_OTR);
        }

        // Configure DTR register as 2 bytes unsigned integer (0 - 65535)
        private void ConfigDTR2Bytes(uint TicksPerRev)
        {
            LS7366_3B_wr[0] = (byte)Commands.LS7366_WRITE | (byte)Registers.LS7366_DTR;
            LS7366_3B_wr[1] = (byte)(TicksPerRev / 256);
            LS7366_3B_wr[2] = (byte)(TicksPerRev % 256);

            Write(LS7366_3B_wr);
        }

        // Write one byte to register
        private void Write1Byte(byte reg)
        {
            LS7366_1B_wr[0] = reg;

            Write(LS7366_1B_wr);
        }

        // Write two bytes to a register
        private void Write2Bytes(byte reg, byte cmd)
        {
            LS7366_2B_wr[0] = reg;
            LS7366_2B_wr[1] = cmd;

            Write(LS7366_2B_wr);
        }

        // Return one byte from a register
        private byte Return1Byte(byte reg)
        {
            LS7366_1B_wr[0] = reg;

            WriteRead(LS7366_1B_wr, LS7366_2B_rd);

            byte b = LS7366_2B_rd[1];
            return b;
        }

        // Return integer in 2 byte mode
        private int Return2Bytes(byte reg)
        {
            LS7366_1B_wr[0] = reg;

            WriteRead(LS7366_1B_wr, LS7366_4B_rd);

            int result = (LS7366_4B_rd[1] * 256) + LS7366_4B_rd[2];
            return result;
        }

        // Return integer in 4 byte mode
        private int Return4Bytes(byte reg)
        {
            LS7366_1B_wr[0] = reg;

            WriteRead(LS7366_1B_wr, LS7366_5B_rd);

            int result = (LS7366_5B_rd[1] * 256 * 256 * 256) + LS7366_5B_rd[2] * 256 * 256 + (LS7366_5B_rd[3] * 256) + LS7366_5B_rd[4];
            return result;
        }

        private void Write(byte[] buffer)
        {
#if USE_SOFTWARE_SPI
            SoftwareSPI_WriteRead(buffer, null);
#else
            spi.Write(buffer);
#endif
        }

        private void WriteRead(byte[] writeBuffer, byte[] readBuffer)
        {
#if USE_SOFTWARE_SPI
            SoftwareSPI_WriteRead(writeBuffer, readBuffer);
#else
            spi.WriteRead(writeBuffer, readBuffer);
#endif
        }

#if USE_SOFTWARE_SPI
        private void SoftwareSPI_WriteRead(byte[] write, byte[] read)
        {
            int readCount = 0;

            if (read != null)
            {
                readCount = read.Length;
                Array.Clear(read, 0, read.Length);
            }

            int loopLen = write.Length < readCount ? readCount : write.Length;

            byte w = 0;

            CS.Write(false);

            // per byte
            for (int len = 0; len < loopLen; len++)
            {
                if (len < write.Length)
                    w = write[len];

                byte mask = 0x80;

                // per bit
                for (int i = 0; i < 8; i++)
                {
                    CLOCK.Write(false);

                    if ((w & mask) == mask)
                        MOSI.Write(true);
                    else
                        MOSI.Write(false);

                    CLOCK.Write(true);

                    if (true == MISO.Read())
                        if (read != null)
                            read[len] |= mask;

                    mask >>= 1;
                }

                MOSI.Write(false);
                CLOCK.Write(false);
            }

            Thread.Sleep(20);
            CS.Write(true);
        }
#endif

        #region Registers, commands and modes

        // LS7366 Commands
        public enum Commands : byte
        {
            LS7366_CLEAR = 0x00, // clear register
            LS7366_READ = 0x40, // read register
            LS7366_WRITE = 0x80, // write register
            LS7366_LOAD = 0xC0, // load register
        }

        // LS7366 Registers
        public enum Registers : byte
        {
            LS7366_MDR0 = 0x08, // select MDR0
            LS7366_MDR1 = 0x10, // select MDR1
            LS7366_DTR = 0x18, // select DTR
            LS7366_CNTR = 0x20, // select CNTR
            LS7366_OTR = 0x28, // select OTR
            LS7366_STR = 0x30, // select STR
        }

        // LS7366 MDR0 Counter modes
        public enum MDR0Mode : byte
        {
            LS7366_MDR0_QUAD0 = 0x00, // none quadrature mode
            LS7366_MDR0_QUAD1 = 0x01, // quadrature x1 mode
            LS7366_MDR0_QUAD2 = 0x02, // quadrature x2 mode
            LS7366_MDR0_QUAD4 = 0x03, // quadrature x4 mode
            LS7366_MDR0_FREER = 0x00, // free run mode
            LS7366_MDR0_SICYC = 0x04, // single cycle count mode
            LS7366_MDR0_RANGE = 0x08, // range limit count mode (0-DTR-0)
            // counting freezes at limits but 
            // resumes on direction reverse
            LS7366_MDR0_MODTR = 0x0C, // modulo-n count (n=DTR both dirs)
            LS7366_MDR0_DIDX = 0x00, // disable index
            LS7366_MDR0_LDCNT = 0x10, // config IDX as load DTR to CNTR
            LS7366_MDR0_RECNT = 0x20, // config IDX as reset CNTR (=0)
            LS7366_MDR0_LDOTR = 0x30, // config IDX as load CNTR to OTR  
            LS7366_MDR0_ASIDX = 0x00, // asynchronous index
            LS7366_MDR0_SYINX = 0x40, // synchronous IDX (if !NQUAD)
            LS7366_MDR0_FFAC1 = 0x00, // filter clock division factor=1
            LS7366_MDR0_FFAC2 = 0x80, // filter clock division factor=2
            LS7366_MDR0_NOFLA = 0x00, // no flags
        }

        // LS7366 MDR1 counter modes
        public enum MDR1Mode : byte
        {
            LS7366_MDR1_4BYTE = 0x00, // 4 byte counter mode
            LS7366_MDR1_3BYTE = 0x01, // 3 byte counter mode
            LS7366_MDR1_2BYTE = 0x02, // 2 byte counter mode
            LS7366_MDR1_1BYTE = 0x03, // 1 byte counter mode
            LS7366_MDR1_ENCNT = 0x00, // enable counting
            LS7366_MDR1_DICNT = 0x04, // disable counting
            LS7366_MDR1_FLIDX = 0x20, // FLAG on IDX (index)
            LS7366_MDR1_FLCMP = 0x40, // FLAG on CMP (compare)
            LS7366_MDR1_FLCY = 0x80, // FLAG on CY (carry)
        }
        #endregion
    }
}
